/*
 * mag_calibration.cpp
 *
 *  Created on: 26.08.2013
 *      Author: amalikov
 */

#include <nuttx/config.h>
#include <math.h>
#include <stdio.h>
#include <poll.h>
#include <fcntl.h>
#include <stdlib.h>
#include <visibility.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mag.h>
#include <mavlink/mavlink_log.h>
#include "mag_calibration.h"
#include <drivers/drv_tone_alarm.h>

#include <uORB/topics/sensor_combined.h>

namespace {
bool setMinMax(float v, float &min, float &max);
void say(int fd, const char *msg, int tone = -1);
}


void do_mag_calibration(int mavlink_fd)
{
	int fd = open(MAG_DEVICE_PATH, O_RDWR);

	// erase old calibration
	mag_scale mscale_null = {0, 1, 0, 1, 0, 1};

	if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
		say(mavlink_fd, "failed to set scale / offsets for mag");
	}

	// calibrate range
	if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) {
		say(mavlink_fd, "FAILED to calibrate scale");
	}

	close(fd);

	say(mavlink_fd, "Rotate around all axes\n", 3);

	int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
	mag_report mag;

	float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
	float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX};

	uint64_t timeout = 10 * 1000 * 1000;	// 10 seconds
	uint64_t last_change = hrt_absolute_time();
	uint64_t notify_timeout = 1 * 1000 * 1000;	// 1 second
	uint64_t last_notify = hrt_absolute_time();

	while (hrt_absolute_time() - last_change < timeout) {
		struct pollfd fds[1];
		fds[0].fd = sub_mag;
		fds[0].events = POLLIN;

		if (poll(fds, 1, 1000) == 0) {
			say(mavlink_fd, "mag cal canceled (timed out)\n");
			break;
		}

		orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);

		bool changed = setMinMax(mag.x, mag_min[0], mag_max[0]);
		changed |= setMinMax(mag.y, mag_min[1], mag_max[1]);
		changed |= setMinMax(mag.z, mag_min[2], mag_max[2]);

		if (changed) {
			char str[64];
			sprintf(str, "x=%.2f y=%.2f z=%.2f\n",
					static_cast<double>((mag_min[0] + mag_max[0]) / 2),
					static_cast<double>((mag_min[1] + mag_max[1]) / 2),
					static_cast<double>((mag_min[2] + mag_max[2]) / 2));
			say(mavlink_fd, str);
			last_change = hrt_absolute_time();
		} else if (hrt_absolute_time() - last_notify > notify_timeout) {
			last_notify = hrt_absolute_time();
			say(mavlink_fd, ".", 4);
		}
	}

	close(sub_mag);

	fd = open(MAG_DEVICE_PATH, O_RDWR);

	struct mag_scale mscale;

	if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale))
		say(mavlink_fd, "FAILED to get scale / offsets for mag");

	mscale.x_offset = (mag_min[0] + mag_max[0]) / 2;
	mscale.y_offset = (mag_min[1] + mag_max[1]) / 2;
	mscale.z_offset = (mag_min[2] + mag_max[2]) / 2;

	if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
		say(mavlink_fd, "FAILED to set scale / offsets for mag");

	close(fd);

	if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
		say(mavlink_fd, "Setting X mag offset failed!\n");
	}

	if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
		say(mavlink_fd, "Setting Y mag offset failed!\n");
	}

	if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
		say(mavlink_fd, "Setting Z mag offset failed!\n");
	}

	if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
		say(mavlink_fd, "Setting X mag scale failed!\n");
	}

	if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
		say(mavlink_fd, "Setting Y mag scale failed!\n");
	}

	if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
		say(mavlink_fd, "Setting Z mag scale failed!\n");
	}

	/* auto-save to EEPROM */
	int save_ret = param_save_default();

	if (save_ret != 0) {
		say(mavlink_fd, "FAILED storing calibration");
	}

	char buf[52];
	sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
		(double)mscale.y_offset, (double)mscale.z_offset);
	say(mavlink_fd, buf);

	sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
		(double)mscale.y_scale, (double)mscale.z_scale);
	say(mavlink_fd, buf);

	say(mavlink_fd, "mag calibration done", 3);
}



namespace {

bool setMinMax(float v, float &min, float &max) {
	bool changed = false;

	if (v < min) {
		min = v;
		changed = true;
	}

	if (v > max) {
		max = v;
		changed = true;
	}

	return changed;
}


void say(int fd, const char *msg, int tone) {
	mavlink_log_info(fd, msg);
	warnx("%s", msg);

	if (tone >= 0) {
		int tone_fd = open("/dev/tone_alarm", O_RDWR);
		ioctl(tone_fd, TONE_SET_ALARM, tone);
		close(tone_fd);
	}
}

}
